from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare

'''
创建一个直接由手柄控制并内置PID控制器的Webots仿真
主要用于测试webots环境和手柄功能
'''

def generate_launch_description():

    inputJS = Node(
        package="input",
        executable="joystick",
        name="js_input",
        remappings=[("control_signal", "/robot1/control_signal")],
        output="screen",
    )

    webotsSim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare("robot_sim"), "launch", "sim.launch.py"]
            )
        )
    )

    view = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare("robot_define"), "launch", "view.launch.py"]
            )
        )
    )
    
    return LaunchDescription([inputJS, webotsSim, view])
